/*
 * Settings.h
 *
 *  Created on: 09.07.2013
 *      Author: OrGeJs
 */

#ifndef SETTINGS_H_
#define SETTINGS_H_

#include <iostream>
#include <sstream>
#include <time.h>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <stdio.h>

#include "../includes/uEye/uEye.h"

using namespace cv;
using namespace std;

void help() {
	cout << "This is a camera calibration sample." << endl
			<< "Usage: calibration configurationFile" << endl
			<< "Near the sample file you'll find the configuration file, which has detailed help of "
					"how to edit it.  It may be any OpenCV supported file format XML/YAML."
			<< endl;
}

class Settings {
public:
	Settings() :
			goodInput(false) {
	}
	enum Pattern {
		NOT_EXISTING, CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID
	};
	enum InputType {
		INVALID, CAMERA, VIDEO_FILE, IMAGE_LIST
	};

	void write(FileStorage& fs) const       //Write serialization for this class
			{
		fs << "{" << "BoardSize_Width" << boardSize.width << "BoardSize_Height"
				<< boardSize.height << "Square_Size" << squareSize
				<< "Calibrate_Pattern" << patternToUse
				<< "Calibrate_NrOfFrameToUse" << nrFrames
				<< "Calibrate_FixAspectRatio" << aspectRatio
				<< "Calibrate_AssumeZeroTangentialDistortion"
				<< calibZeroTangentDist
				<< "Calibrate_FixPrincipalPointAtTheCenter"
				<< calibFixPrincipalPoint

				<< "Write_DetectedFeaturePoints" << bwritePoints
				<< "Write_extrinsicParameters" << bwriteExtrinsics
				<< "Write_outputFileName" << outputFileName

				<< "Show_UndistortedImage" << showUndistorsed

				<< "Input_FlipAroundHorizontalAxis" << flipVertical
				<< "Input_Delay" << delay << "Input" << input << "}";
	}
	void read(const FileNode& node)          //Read serialization for this class
			{
		node["BoardSize_Width"] >> boardSize.width;
		node["BoardSize_Height"] >> boardSize.height;
		node["Calibrate_Pattern"] >> patternToUse;
		node["Square_Size"] >> squareSize;
		node["Calibrate_NrOfFrameToUse"] >> nrFrames;
		node["Calibrate_FixAspectRatio"] >> aspectRatio;
		node["Write_DetectedFeaturePoints"] >> bwritePoints;
		node["Write_extrinsicParameters"] >> bwriteExtrinsics;
		node["Write_outputFileName"] >> outputFileName;
		node["Calibrate_AssumeZeroTangentialDistortion"]
				>> calibZeroTangentDist;
		node["Calibrate_FixPrincipalPointAtTheCenter"]
				>> calibFixPrincipalPoint;
		node["Input_FlipAroundHorizontalAxis"] >> flipVertical;
		node["Show_UndistortedImage"] >> showUndistorsed;
		node["Input"] >> input;
		node["Input_Delay"] >> delay;
		interprate();
	}
	void interprate() {
		goodInput = true;
		if (boardSize.width <= 0 || boardSize.height <= 0) {
			cerr << "Invalid Board size: " << boardSize.width << " "
					<< boardSize.height << endl;
			goodInput = false;
		}
		if (squareSize <= 10e-6) {
			cerr << "Invalid square size " << squareSize << endl;
			goodInput = false;
		}
		if (nrFrames <= 0) {
			cerr << "Invalid number of frames " << nrFrames << endl;
			goodInput = false;
		}

		if (input.empty())      // Check for valid input
			inputType = INVALID;
		else {
			if (input[0] >= '0' && input[0] <= '9') {
				stringstream ss(input);
				ss >> cameraID;
				inputType = CAMERA;
			} else {
				if (readStringList(input, imageList)) {
					inputType = IMAGE_LIST;
					nrFrames =
							((unsigned int) nrFrames < imageList.size()) ?
									nrFrames : imageList.size();
				} else
					inputType = VIDEO_FILE;
			}
			if (inputType == CAMERA)
				inputCapture.open(cameraID);
			if (inputType == VIDEO_FILE)
//				inputCapture.open(input);
			if (inputType != IMAGE_LIST && !inputCapture.isOpened())
				inputType = INVALID;
		}
		if (inputType == INVALID) {
			cerr << " Inexistent input: " << input;
			goodInput = false;
		}

		flag = 0;
		if (calibFixPrincipalPoint)
			flag |= CV_CALIB_FIX_PRINCIPAL_POINT;
		if (calibZeroTangentDist)
			flag |= CV_CALIB_ZERO_TANGENT_DIST;
		if (aspectRatio)
			flag |= CV_CALIB_FIX_ASPECT_RATIO;

		calibrationPattern = NOT_EXISTING;
		if (!patternToUse.compare("CHESSBOARD"))
			calibrationPattern = CHESSBOARD;
		if (!patternToUse.compare("CIRCLES_GRID"))
			calibrationPattern = CIRCLES_GRID;
		if (!patternToUse.compare("ASYMMETRIC_CIRCLES_GRID"))
			calibrationPattern = ASYMMETRIC_CIRCLES_GRID;
		if (calibrationPattern == NOT_EXISTING) {
			cerr << " Inexistent camera calibration mode: " << patternToUse
					<< endl;
			goodInput = false;
		}
		atImageList = 0;

	}
	Mat nextImage() {
		Mat result;
		if (inputCapture.isOpened()) {
			Mat view0;
			inputCapture >> view0;
			view0.copyTo(result);
		} else if (atImageList < (int) imageList.size())
			result = imread(imageList[atImageList++], CV_LOAD_IMAGE_COLOR);

		return result;
	}

	static bool readStringList(const string& filename, vector<string>& l) {
		l.clear();
		FileStorage fs(filename, FileStorage::READ);
		if (!fs.isOpened())
			return false;
		FileNode n = fs.getFirstTopLevelNode();
		if (n.type() != FileNode::SEQ)
			return false;
		FileNodeIterator it = n.begin(), it_end = n.end();
		for (; it != it_end; ++it)
			l.push_back((string) *it);
		return true;
	}
public:
	Size boardSize; // The size of the board -> Number of items by width and height
	Pattern calibrationPattern; // One of the Chessboard, circles, or asymmetric circle pattern
	float squareSize; // The size of a square in your defined unit (point, millimeter,etc).
	int nrFrames;  // The number of frames to use from the input for calibration
	float aspectRatio;         // The aspect ratio
	int delay;                 // In case of a video input
	bool bwritePoints;         //  Write detected feature points
	bool bwriteExtrinsics;     // Write extrinsic parameters
	bool calibZeroTangentDist; // Assume zero tangential distortion
	bool calibFixPrincipalPoint; // Fix the principal point at the center
	bool flipVertical;    // Flip the captured images around the horizontal axis
	string outputFileName;      // The name of the file where to write
	bool showUndistorsed;       // Show undistorted images after calibration
	string input;               // The input ->

	int cameraID;
	vector<string> imageList;
	int atImageList;
	VideoCapture inputCapture;
	InputType inputType;
	bool goodInput;
	int flag;

private:
	string patternToUse;

};
void write(FileStorage& fs, const std::string&, const Settings& x) {
	x.write(fs);
}
void read(const FileNode& node, Settings& x, const Settings& default_value =
		Settings()) {
	if (node.empty())
		x = default_value;
	else
		x.read(node);
}

enum {
	DETECTION = 0, CAPTURING = 1, CALIBRATED = 2
};
double computeReprojectionErrors(const vector<vector<Point3f> >& objectPoints,
		const vector<vector<Point2f> >& imagePoints, const vector<Mat>& rvecs,
		const vector<Mat>& tvecs, const Mat& cameraMatrix,
		const Mat& distCoeffs, vector<float>& perViewErrors) {
	vector<Point2f> imagePoints2;
	int i, totalPoints = 0;
	double totalErr = 0, err;
	perViewErrors.resize(objectPoints.size());

	for (i = 0; i < (int) objectPoints.size(); ++i) {
		projectPoints(Mat(objectPoints[i]), rvecs[i], tvecs[i], cameraMatrix,
				distCoeffs, imagePoints2);
		err = norm(Mat(imagePoints[i]), Mat(imagePoints2), CV_L2);

		int n = (int) objectPoints[i].size();
		perViewErrors[i] = (float) std::sqrt(err * err / n);
		totalErr += err * err;
		totalPoints += n;
	}

	return std::sqrt(totalErr / totalPoints);
}

void calcBoardCornerPositions(Size boardSize, float squareSize,
		vector<Point3f>& corners,
		Settings::Pattern patternType /*= Settings::CHESSBOARD*/) {
	corners.clear();

	switch (patternType) {
	case Settings::CHESSBOARD:
	case Settings::CIRCLES_GRID:
		for (int i = 0; i < boardSize.height; ++i)
			for (int j = 0; j < boardSize.width; ++j)
				corners.push_back(
						Point3f(float(j * squareSize), float(i * squareSize),
								0));
		break;

	case Settings::ASYMMETRIC_CIRCLES_GRID:
		for (int i = 0; i < boardSize.height; i++)
			for (int j = 0; j < boardSize.width; j++)
				corners.push_back(
						Point3f(float((2 * j + i % 2) * squareSize),
								float(i * squareSize), 0));
		break;

	case Settings::NOT_EXISTING:

		break;
	}
}

bool runCalibration(Settings& s, Size& imageSize, Mat& cameraMatrix,
		Mat& distCoeffs, vector<vector<Point2f> > imagePoints,
		vector<Mat>& rvecs, vector<Mat>& tvecs, vector<float>& reprojErrs,
		double& totalAvgErr) {

	cameraMatrix = Mat::eye(3, 3, CV_64F);
	if (s.flag & CV_CALIB_FIX_ASPECT_RATIO)
		cameraMatrix.at<double>(0, 0) = 1.0;

	distCoeffs = Mat::zeros(8, 1, CV_64F);

	vector<vector<Point3f> > objectPoints(1);
	calcBoardCornerPositions(s.boardSize, s.squareSize, objectPoints[0],
			s.calibrationPattern);

	objectPoints.resize(imagePoints.size(), objectPoints[0]);

	//Find intrinsic and extrinsic camera parameters
	double rms = calibrateCamera(objectPoints, imagePoints, imageSize,
			cameraMatrix, distCoeffs, rvecs, tvecs,
			s.flag | CV_CALIB_FIX_K4 | CV_CALIB_FIX_K5);

	cout << "Re-projection error reported by calibrateCamera: " << rms << endl;

	bool ok = checkRange(cameraMatrix) && checkRange(distCoeffs);

	totalAvgErr = computeReprojectionErrors(objectPoints, imagePoints, rvecs,
			tvecs, cameraMatrix, distCoeffs, reprojErrs);

	return ok;
}

// Print camera parameters to the output file
void saveCameraParams(Settings& s, Size& imageSize, Mat& cameraMatrix,
		Mat& distCoeffs, const vector<Mat>& rvecs, const vector<Mat>& tvecs,
		const vector<float>& reprojErrs,
		const vector<vector<Point2f> >& imagePoints, double totalAvgErr) {
	FileStorage fs(s.outputFileName, FileStorage::WRITE);

	time_t t;
	time(&t);
	struct tm *t2 = localtime(&t);
	char buf[1024];
	strftime(buf, sizeof(buf) - 1, "%c", t2);

	fs << "calibration_Time" << buf;

	if (!rvecs.empty() || !reprojErrs.empty())
		fs << "nrOfFrames" << (int) std::max(rvecs.size(), reprojErrs.size());
	fs << "image_Width" << imageSize.width;
	fs << "image_Height" << imageSize.height;
	fs << "board_Width" << s.boardSize.width;
	fs << "board_Height" << s.boardSize.height;
	fs << "square_Size" << s.squareSize;

	if (s.flag & CV_CALIB_FIX_ASPECT_RATIO)
		fs << "FixAspectRatio" << s.aspectRatio;

	if (s.flag) {
		sprintf(buf, "flags: %s%s%s%s",
				s.flag & CV_CALIB_USE_INTRINSIC_GUESS ?
						" +use_intrinsic_guess" : "",
				s.flag & CV_CALIB_FIX_ASPECT_RATIO ? " +fix_aspectRatio" : "",
				s.flag & CV_CALIB_FIX_PRINCIPAL_POINT ?
						" +fix_principal_point" : "",
				s.flag & CV_CALIB_ZERO_TANGENT_DIST ?
						" +zero_tangent_dist" : "");
		cvWriteComment(*fs, buf, 0);

	}

	fs << "flagValue" << s.flag;

	fs << "Camera_Matrix" << cameraMatrix;
	fs << "Distortion_Coefficients" << distCoeffs;

	fs << "Avg_Reprojection_Error" << totalAvgErr;
	if (!reprojErrs.empty())
		fs << "Per_View_Reprojection_Errors" << Mat(reprojErrs);

	if (!rvecs.empty() && !tvecs.empty()) {
		CV_Assert(rvecs[0].type() == tvecs[0].type());
		Mat bigmat((int) rvecs.size(), 6, rvecs[0].type());
		for (int i = 0; i < (int) rvecs.size(); i++) {
			Mat r = bigmat(Range(i, i + 1), Range(0, 3));
			Mat t = bigmat(Range(i, i + 1), Range(3, 6));

			CV_Assert(rvecs[i].rows == 3 && rvecs[i].cols == 1);
			CV_Assert(tvecs[i].rows == 3 && tvecs[i].cols == 1);
			//*.t() is MatExpr (not Mat) so we can use assignment operator
			r = rvecs[i].t();
			t = tvecs[i].t();
		}
		cvWriteComment(*fs,
				"a set of 6-tuples (rotation vector + translation vector) for each view",
				0);
		fs << "Extrinsic_Parameters" << bigmat;
	}

	if (!imagePoints.empty()) {
		Mat imagePtMat((int) imagePoints.size(), imagePoints[0].size(),
				CV_32FC2);
		for (int i = 0; i < (int) imagePoints.size(); i++) {
			Mat r = imagePtMat.row(i).reshape(2, imagePtMat.cols);
			Mat imgpti(imagePoints[i]);
			imgpti.copyTo(r);
		}
		fs << "Image_points" << imagePtMat;
	}
}

bool runCalibrationAndSave(Settings& s, Size imageSize, Mat& cameraMatrix,
		Mat& distCoeffs, vector<vector<Point2f> > imagePoints) {
	vector<Mat> rvecs, tvecs;
	vector<float> reprojErrs;
	double totalAvgErr = 0;

	bool ok = runCalibration(s, imageSize, cameraMatrix, distCoeffs,
			imagePoints, rvecs, tvecs, reprojErrs, totalAvgErr);
	cout << (ok ? "Calibration succeeded" : "Calibration failed")
			<< ". avg re projection error = " << totalAvgErr;

	if (ok)
		saveCameraParams(s, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs,
				reprojErrs, imagePoints, totalAvgErr);
	return ok;
}

#endif /* SETTINGS_H_ */
